void init_mtree()        // initialize structures
{ 
  tree_busy = 0; trees_built = 0;
  phase_start = 0; peer_discovery = 1;
  memset(trees, 0, sizeof(trees));
  init_trees();

#ifdef TOSSIM
  if (config.load_trees)
  {
    DBGOUT("Loading trees...");
    alloc_treesummaries();
    load_trees();
    tree_phase = TREE_COUNT;
    peer_discovery = 0;
    tree_updated = 0;
  } else {
    DBGOUT("Loading values...");
    load_values();
  }
#endif
}

void start_treeconstruct()
{
#ifndef TOSSIM
  int i;
#endif

  DBGOUT("Starting tree construction");
  phase_start = timer_cycles; tree_phase = 0; treemsg_sending = 0; peer_discovery = 0;
  if (TOS_NODE_ID == BASESTATION_ADDR)     // start tree construction
  {
    trees[0].hops = 0;
    tree_updated = 1;
    tree_busy = 1;
    trees_built = 0;
  }

#ifndef TOSSIM                  // sample static hardware attributes on tree rebuild
  for (i = 0; i < MAX_ATTRDESC; i++)
    if (attr_desc[i].is_hardware && attr_desc[i].is_dynamic == 0)
      sample_attr(i);
#endif
}

void init_trees()
{
  int i;

  for (i = 0; i < TREE_COUNT; i++)
  {
    trees[i].parent = 0xffff; trees[i].hops = 0xff;
    trees[i].deepest_id = 0xffff; trees[i].deepestsum_id = 0xffff;
    trees[i].deepest_hops = 0xff; trees[i].deepesthops_sum = 0xff;
    trees[i].uncommitted_children = 0;
  }
  
  if (TOS_NODE_ID == BASESTATION_ADDR)
    trees[0].hops = 0;
}

void alloc_treesummaries()
{
  int i, j;

  for (i = 0; i < TREE_COUNT; i++)
  {
    trees[i].summary = (SUMMARY **) mcalloc(sizeof(SUMMARY *), summarydesc_count);
    for (j = 0; j < summarydesc_count; j++)
    {
      trees[i].summary[j] = (SUMMARY *) mmalloc(sizeof(SUMMARY));
      summary_init(trees[i].summary[j], summary_desc[j].type, summary_desc[j].options);
    }
  }
}

void reset_treesummaries()
{
  int i, j, k;

  init_trees();
  for (i = 0; i < TREE_COUNT; i++)
  {    
    if (trees[i].summary)
    {
      for (k = 0; k < summarydesc_count; k++)
        mfree(trees[i].summary[k]);
      mfree(trees[i].summary);
      trees[i].summary = NULL;
    }
    
    for (j = 0; j < MAX_PEERLINKS; j++)
      if (peer_link[j].used)
        if (peer_link[j].tree.trees[i].summary)
        {
          for (k = 0; k < summarydesc_count; k++)
            if (peer_link[j].tree.trees[i].summary[k])
              mfree(peer_link[j].tree.trees[i].summary[k]);

          mfree(peer_link[j].tree.trees[i].summary);
          peer_link[j].tree.trees[i].summary = NULL;
        }
  }
}

int list_children(ADDRESS **nodes, uint8_t *node_count, int tree)
{                            // return ids of neighbors which are children in a given tree
  int i;
  ADDRESS n[MAX_PEERLINKS];

  if (tree < 0 || tree >= TREE_COUNT)
    return -1;

  *node_count = 0;
  for (i = 0; i < MAX_PEERLINKS; i++)
    if (peer_link[i].used && peer_link[i].tree.trees[tree].parent == TOS_NODE_ID)
      n[(*node_count)++] = peer_link[i].id;

  if (*node_count > 0)
  {
    (*nodes) = (ADDRESS *) mcalloc(sizeof(ADDRESS), *node_count);
    memcpy(*nodes, n, sizeof(ADDRESS)*(*node_count));
  } else (*nodes) = NULL;
    
  return 0;
}

int8_t list_peers(ADDRESS **nodes, uint8_t *node_count)
{                             // return ids of all neighbors
  int i;
  ADDRESS n[MAX_PEERLINKS];

  *node_count = 0;
  for (i = 0; i < MAX_PEERLINKS; i++)
    if (peer_link[i].used)
      n[(*node_count)++] = peer_link[i].id;
  
  if (*node_count > 0)
  {
    (*nodes) = (ADDRESS *) mcalloc(sizeof(ADDRESS), *node_count);
    memcpy(*nodes, n, sizeof(ADDRESS)*(*node_count));
  } else (*nodes) = NULL;

  return 0;
}

void send_treeconstructmsg(uint8_t tree)  
{                          // marshal and send out a tree construct message for a given tree
  TREECONSTRUCT_MSG msg;
  uint8_t buffer[sizeof(msg)], len;
  ADDRESS *nodes;
  uint8_t node_count;

  set_globalstatus(GS_TREEBUILD);
  treemsg_sending = 1;

  msg.cmd = CMD_TREECONSTRUCT;
  msg.tree = tree;
  msg.parent = trees[tree].parent;
  msg.hops = trees[tree].hops;
  msg.phase_start = phase_start;

  list_peers(&nodes, &node_count);
  len = marshal_treeconstructmsg(&msg, buffer);
  send_genericmsg(buffer, len, MSG_CONFIRM, node_count, nodes, -1, PEER_DEFAULTPOWER, 1);
}

void send_treecommitmsg()
{                    // marshal and send out a tree commit message for a given tree, to a given neighbor
  TREECOMMIT_MSG msg;
  uint8_t buffer[sizeof(msg)], len;
  ADDRESS *nodes;
  uint8_t node_count;

  DBGOUT("Committing tree %d...", tree_phase);
  msg.cmd = CMD_TREECOMMIT;
  msg.tree = tree_phase;

  list_children(&nodes, &node_count, tree_phase);
  len = marshal_treecommitmsg(&msg, buffer);
  send_genericmsg(buffer, len, MSG_CONFIRM, node_count, nodes, -1, PEER_DEFAULTPOWER, 1);
}

void send_treesummarymsg(uint8_t to_uart)
{                    // marshal and send out a tree summary message for a given tree, to the parent of the tree
  int i;
  TREESUMMARY_MSG msg;
  uint8_t buffer[sizeof(msg)], len;

  msg.cmd = CMD_TREESUMMARY;
  msg.tree = tree_phase;
  msg.deepest_id = trees[tree_phase].deepest_id;
  msg.deepestsum_id = trees[tree_phase].deepestsum_id;
  msg.deepest_hops = trees[tree_phase].deepest_hops;
  msg.deepesthops_sum = trees[tree_phase].deepesthops_sum;

  for (i = 0; i < summarydesc_count; i++)
    memcpy(&msg.summary[i], trees[tree_phase].summary[i], sizeof(SUMMARY));

  len = marshal_treesummarymsg(&msg, buffer);
  if (to_uart)
  {
    DBGOUT("Sending summary to UART");
    enqueue_uartmsg(1, len, buffer, TOS_NODE_ID);
  } else {
    DBGOUT("Sending summary to node %d in tree %d...", trees[tree_phase].parent, tree_phase);
    send_genericmsg(buffer, len, MSG_CONFIRM, 1, alloc_addr(trees[tree_phase].parent), -1, PEER_DEFAULTPOWER, 1);
  }
}

void send_treesummaryupdatemsg(TREESUMMARYUPDATE_MSG *msg)
{
  int size;
  uint8_t buffer[sizeof(TREESUMMARYUPDATE_MSG)];

  DBGOUT("Sending update to parent %d", trees[msg->tree_index].parent);
  msg->cmd = CMD_TREESUMMARYUPDATE;
  size = marshal_treesummaryupdatemsg(msg, buffer);
  send_genericmsg(buffer, size, MSG_CONFIRM, 1, alloc_addr(trees[msg->tree_index].parent), -1, PEER_DEFAULTPOWER, 1);
}

void send_treerootnotifymsg(ADDRESS notify_id)
{                       // marshal and send out a tree root notify message for a given tree, to children in this tree
  TREEROOTNOTIFY_MSG msg;
  uint8_t buffer[sizeof(msg)], len;
  uint8_t node_count;
  ADDRESS *nodes;
  int i;
  ADDRESS n[MAX_PEERLINKS];

  DBGOUT("Sending notify message in tree %d for node %d", tree_phase, notify_id);
  msg.cmd = CMD_TREEROOTNOTIFY;
  msg.tree = tree_phase;
  msg.notify_id = notify_id;

  node_count = 0;
  for (i = 0; i < MAX_PEERLINKS; i++)
    if (peer_link[i].used && peer_link[i].tree.trees[tree_phase-1].parent == TOS_NODE_ID &&
      summary_test(peer_link[i].tree.trees[tree_phase-1].summary[ATTR_ID], notify_id, 0))
      n[node_count++] = peer_link[i].id;

  if (node_count > 0)
  {
    nodes = (ADDRESS *) mcalloc(node_count, sizeof(ADDRESS));
    memcpy(nodes, n, sizeof(ADDRESS)*node_count);
  } else {
    nodes = NULL;

    if (trees[tree_phase-1].hops == 0)
    {
      DBGERR("Root cannot find any children to forward notification!");
      pause_sim();
      return;
    }
  }

  if (node_count > 0)
  {
    len = marshal_treerootnotifymsg(&msg, buffer);
    send_genericmsg(buffer, len, MSG_CONFIRM, node_count, nodes, -1, PEER_DEFAULTPOWER, 1);
  }
}

int child_multiplicity(ADDRESS node)   // how many times a node is a child in existing trees
{
  int k, t, count = 0;

  for (k = 0; k < MAX_PEERLINKS; k++)
    if (peer_link[k].used && peer_link[k].id == node)
      for (t = 0; t < tree_phase; t++)
        if (peer_link[k].tree.trees[t].parent == TOS_NODE_ID)
          count++;

  return count;
}

uint8_t better_parent(uint8_t hops, ADDRESS id)
{                            // returns true if the supplied parent and hops are better than the current ones
  int t;

  if (trees[tree_phase].hops == 0xff)
    return 1;
  if (trees[tree_phase].hops > hops+1)
    return 1;
  else if (trees[tree_phase].hops < hops+1)
    return 0;

  for (t = 0; t <= tree_phase; t++)
    if (trees[t].parent == id)
      return 0;

  for (t = 0; t < tree_phase; t++)
    if (trees[tree_phase].parent == trees[t].parent)
      return 1;

  return (child_multiplicity(id) < child_multiplicity(trees[tree_phase].parent));
}

void handle_treeconstructmsg(ADDRESS source_id, uint8_t *p)
{                       // handles tree construction logical message
  int peer_index;
  TREECONSTRUCT_MSG msg;

  unmarshal_treeconstructmsg(&msg, p);

  peer_index = id_to_peerlink(source_id);
  if (peer_index < 0)
  {
    DBGERR("Invalid neighbor id: %d", source_id);
    return;
  }

  if (msg.tree == 0 && (tree_phase != 0 || trees[0].hops == 0xff))
  {
    start_treeconstruct();
    reset_treesummaries();
  } else if (tree_phase != msg.tree)
  {
    DBGERR("Invalid tree phase: %d", msg.tree);
    pause_sim();
    return;
  } else if (trees[tree_phase].hops == 0xff)
  {
    DBGOUT("Starting new tree consturction");
    phase_start = msg.phase_start;
  }

  peer_link[peer_index].tree.trees[tree_phase].parent = msg.parent;
  peer_link[peer_index].tree.trees[tree_phase].hops = msg.hops;
  if (better_parent(msg.hops, source_id) && timer_cycles < phase_start+TREEEPOCHUPDATE_COUNT)
  {
    DBGOUT("Selecting %d as parent in tree %d (%d hops)", source_id, tree_phase, msg.hops+1);
    trees[tree_phase].parent = source_id;
    trees[tree_phase].hops = msg.hops+1;
    tree_updated = 1;
    treeupdate_time = timer_cycles;
  }
}

void handlesent_treeconstructmsg(uint8_t msg_len, uint8_t *p, uint8_t send_success,
  uint8_t node_count, ADDRESS *nodes, uint8_t confirm_seq)
{                         // allow only one tree construct message to be queued up at a time. After confirmation, clear flag.
  treemsg_sending = 0;
}

void do_commit()      // all the children in the current tree have committed, air a commit message to parent
{
  int i, j;
  uint8_t cmd;

  set_globalstatus(GS_IDLE);

  for (i = 0; i < summarydesc_count; i++)       // add my summary to the aggregated summary of the subtree
  {
#ifdef TOSSIM  
    DBGOUT("Adding value of %d for attribute %s to summary %d",
      attr_desc[summary_desc[i].attrdesc_index].value, attr_name[summary_desc[i].attrdesc_index], i);
#else
    DBGOUT("Adding value of %d for attribute %d to summary %d",
      attr_desc[summary_desc[i].attrdesc_index].value, summary_desc[i].attrdesc_index, i);
#endif      

    summary_add(trees[tree_phase].summary[i], attr_desc[summary_desc[i].attrdesc_index].value);
    if (summary_desc[i].attrdesc_index == ATTR_ID && summary_desc[i].type == SUMMARY_BLOOMFILTER &&
      summary_desc[i].options == 1)      // if we are doing BF on IDs, and option is 1, add neighbors (for RepairRoute)
      for (j = 0; j < MAX_PEERLINKS; j++)
        if (peer_link[j].used && peerlink_statusok(j) && peer_link[j].id != trees[tree_phase].parent &&
          peer_link[j].tree.trees[tree_phase].parent != TOS_NODE_ID)
        {
#ifdef TOSSIM
          DBGOUT("Adding value of %d for attribute %s to summary %d", peer_link[j].id, attr_name[ATTR_ID], i);
#else
          DBGOUT("Adding value of %d for attribute %d to summary %d", peer_link[j].id, ATTR_ID, i);
#endif          
          summary_add(trees[tree_phase].summary[i], peer_link[j].id);
        }
  }

  DBGOUT("Sending summaries:");
  for (i = 0; i < summarydesc_count; i++)
    summary_print(trees[tree_phase].summary[i]);

#ifdef TOSSIM    
  if (tree_phase == TREE_COUNT-1)
    store_trees();
#endif

  if (trees[tree_phase].hops > 0)     // check if I am root
  {
    DBGOUT("Committed tree %d to parent %d", tree_phase, trees[tree_phase].parent);
    send_treesummarymsg(0);
    tree_phase++;
  } else {
    DBGOUT("Root committed!");
    if (TOS_NODE_ID == BASESTATION_ADDR && tree_phase == 0)
      send_treesummarymsg(1);
      
    tree_phase++;
    if (tree_phase < TREE_COUNT)
      if (trees[tree_phase-1].deepestsum_id != TOS_NODE_ID)
      {
        DBGOUT("Notifying %d as the new root for tree %d", trees[tree_phase-1].deepestsum_id,
          tree_phase);
        send_treerootnotifymsg(trees[tree_phase-1].deepestsum_id);
      } else {
        DBGOUT("Starting new tree construction");
        trees[tree_phase].hops = 0;
        tree_updated = 1;
        phase_start = timer_cycles;
      }
    else {
      DBGOUT("Tree construction done. Notifying base station");
      cmd = CMDHI_TREESDONE;
      init_forward(TOS_NODE_ID, TOS_NODE_ID, BASESTATION_ADDR, 0, NULL, FORWARD_DESTRUCTIVE, 1, &cmd,
        0, NULL, ROUTE_SUCCESS);
    }
  }
}

void init_commit()          // just received message from parent to begin committing trees
{                           // send a commit message to each child in sequence
  int i;

  set_globalstatus(GS_TREECOMMIT);

  trees[tree_phase].deepest_id = TOS_NODE_ID;
  trees[tree_phase].deepestsum_id = TOS_NODE_ID;
  trees[tree_phase].deepest_hops = trees[tree_phase].hops;
  trees[tree_phase].deepesthops_sum = 0;
  for (i = 0; i <= tree_phase; i++)
    trees[tree_phase].deepesthops_sum += trees[i].hops;

  trees[tree_phase].uncommitted_children = 0;
  for (i = 0; i < MAX_PEERLINKS; i++)
    if (peer_link[i].used && peer_link[i].tree.trees[tree_phase].parent == TOS_NODE_ID)
      trees[tree_phase].uncommitted_children++;

  if (trees[tree_phase].uncommitted_children > 0)
    send_treecommitmsg();
  else do_commit();
}

void handle_treecommitmsg(ADDRESS source_id, uint8_t *p)
{                        // handler to tree commit logical message
  TREECOMMIT_MSG msg;
  
  DBGOUT("Tree commit message received from %d", source_id);
  unmarshal_treecommitmsg(&msg, p);
  
  if (msg.tree != tree_phase || source_id != trees[msg.tree].parent) 
  {
    DBGOUT("Ignored tree commit (%hu, %hu, %u, %u)", msg.tree, tree_phase, source_id, trees[msg.tree].parent);  
    return;
  }
  
  init_commit();
}

uint8_t checkaccept_treecommit(ADDRESS source_id, uint8_t *p)   // accept tree commit only if nothing is currenly sampling (wait for static attrs)
{
  return (sensor_needsampling() == 0);
}

void handle_treesummarymsg(ADDRESS source_id, uint8_t *p)
{                            // handler for tree summary logical message
  int i, peer_index;
  TREESUMMARY_MSG msg;

  unmarshal_treesummarymsg(&msg, p);
  if (msg.tree != tree_phase)
  {
    DBGERR("Invalid tree phase: %d, %d", msg.tree, tree_phase);
    return;
  }
  if (trees[tree_phase].uncommitted_children == 0)
  {
    DBGERR("Already committed all children in tree %d", tree_phase);
    pause_sim();
    return;
  }

  DBGOUT("Summaries received from %d for tree %d", source_id, msg.tree);
  for (i = 0; i < summarydesc_count; i++)
    summary_print(&msg.summary[i]);

  peer_index = id_to_peerlink(source_id);
  if (peer_index < 0 || peer_link[peer_index].tree.trees[msg.tree].parent != TOS_NODE_ID)
  {
    DBGERR("Node %d not a child in tree %d!", source_id, msg.tree);
    return;
  }
                                           // maintain recursively deepest node
  if (msg.deepest_hops > trees[tree_phase].deepest_hops ||
    (msg.deepest_hops == trees[tree_phase].deepest_hops && msg.deepest_id != TOS_NODE_ID))
  {
    trees[tree_phase].deepest_id = msg.deepest_id;
    trees[tree_phase].deepest_hops = msg.deepest_hops;
  }
                                           // maintain new root candidate
  if (msg.deepesthops_sum > trees[tree_phase].deepesthops_sum ||
    (msg.deepesthops_sum == trees[tree_phase].deepesthops_sum &&
     msg.deepestsum_id != TOS_NODE_ID))
  {
    DBGOUT("New deepest id: %d, hops: %d", msg.deepestsum_id, msg.deepesthops_sum);
    trees[tree_phase].deepestsum_id = msg.deepestsum_id;
    trees[tree_phase].deepesthops_sum = msg.deepesthops_sum;
  }

  for (i = 0; i < summarydesc_count; i++)        // add this peer's summary to the aggregated
    summary_merge(trees[tree_phase].summary[i], &msg.summary[i]);

  DBGOUT("Aggregated summaries:");
  for (i = 0; i < summarydesc_count; i++)
    summary_print(trees[tree_phase].summary[i]);

  peer_link[peer_index].tree.trees[tree_phase].deepest_hops = msg.deepest_hops;
  if (peer_link[peer_index].tree.trees[tree_phase].summary == NULL)
  {
    peer_link[peer_index].tree.trees[tree_phase].summary =
      (SUMMARY **) mcalloc(sizeof(SUMMARY *), summarydesc_count);
    memset(peer_link[peer_index].tree.trees[tree_phase].summary, 0, summarydesc_count*sizeof(SUMMARY *));
  }

  for (i = 0; i < summarydesc_count; i++)
  {
    if (peer_link[peer_index].tree.trees[tree_phase].summary[i] == NULL)
      peer_link[peer_index].tree.trees[tree_phase].summary[i] = (SUMMARY *) mmalloc(sizeof(SUMMARY));
    memcpy(peer_link[peer_index].tree.trees[tree_phase].summary[i], &msg.summary[i], sizeof(SUMMARY));
  }

  trees[tree_phase].uncommitted_children--;
  DBGOUT("Children remaining: %d", trees[tree_phase].uncommitted_children);
  
  if (trees[tree_phase].uncommitted_children == 0)
    do_commit();
}

void handle_treesummaryupdatemsg(ADDRESS source_id, uint8_t *p)
{
  TREESUMMARYUPDATE_MSG msg;
  int i, peer_index, diff_len;
  SUMMARY summary;

  unmarshal_treesummaryupdatemsg(&msg, p);
  DBGOUT("Tree summary update message received from %d", source_id);
  DBGOUT("Source node: %d, New parent: %d", msg.source_node, msg.new_parentnode);
  DBGOUT("Is insert: %d, is diff: %d, summary: %d, seq_number: %d, tree: %d", msg.is_insert,
    msg.is_diff, msg.summary_index, msg.seq_number, msg.tree_index);

  peer_index = id_to_peerlink(source_id);
  if (peer_index < 0)
  {
    DBGERR("Invalid peer index: %d", peer_index);
    pause_sim();
    return;
  }

  if (msg.seq_number > peer_link[peer_index].tree.trees[msg.tree_index].update_seqnumber)
  {
    peer_link[peer_index].tree.trees[msg.tree_index].update_seqnumber = msg.seq_number;
    if (msg.is_insert)
    {
      memcpy(&summary, trees[msg.tree_index].summary[msg.summary_index], sizeof(SUMMARY));
      summary_add(&summary, msg.value);
      if (peer_link[peer_index].id == msg.source_node)
      {
        peer_link[peer_index].tree.trees[msg.tree_index].parent = TOS_NODE_ID;

        if (peer_link[peer_index].tree.trees[msg.tree_index].summary == NULL)
        {
          peer_link[peer_index].tree.trees[msg.tree_index].summary =
            (SUMMARY **) mcalloc(sizeof(SUMMARY *), summarydesc_count);
          memset(peer_link[peer_index].tree.trees[msg.tree_index].summary, 0,
            summarydesc_count*sizeof(SUMMARY *));
        }

        if (peer_link[peer_index].tree.trees[msg.tree_index].summary[msg.summary_index] == NULL)
        {
          peer_link[peer_index].tree.trees[msg.tree_index].summary[msg.summary_index] =
            (SUMMARY *) mmalloc(sizeof(SUMMARY));
          summary_init(peer_link[peer_index].tree.trees[msg.tree_index].summary[msg.summary_index],
            summary_desc[msg.summary_index].type, summary_desc[msg.summary_index].options);
        }
      }

      summary_add(peer_link[peer_index].tree.trees[msg.tree_index].summary[msg.summary_index], msg.value);
    } else {
      if (msg.is_diff)
        decode_bitdiff((uint8_t *) peer_link[peer_index].tree.trees[msg.tree_index].summary[msg.summary_index],
          (uint8_t *) &msg.summary, sizeof(SUMMARY), msg.diff, msg.diff_len);

      summary_init(&summary, summary_desc[msg.summary_index].type,
        summary_desc[msg.summary_index].options);
      summary_merge(&summary, &msg.summary);
      memcpy(peer_link[peer_index].tree.trees[msg.tree_index].summary[msg.summary_index], &msg.summary,
        sizeof(SUMMARY));

      for (i = 0; i < MAX_PEERLINKS; i++)
        if (peer_link[i].used && peer_link[i].tree.trees[msg.tree_index].parent == TOS_NODE_ID)
        {
          if (i == peer_index)
            peer_link[i].tree.trees[msg.tree_index].parent = msg.new_parentnode;
          else summary_merge(&summary, peer_link[i].tree.trees[msg.tree_index].summary[msg.summary_index]);
        }  
      summary_add(&summary, attr_desc[summary_desc[msg.summary_index].attrdesc_index].value);
    }

    if (summary_equal(&summary, trees[msg.tree_index].summary[msg.summary_index]) == 0)
    {
      DBGOUT("New summary");
      summary_print(&summary);
      DBGOUT("Old summary");
      summary_print(trees[msg.tree_index].summary[msg.summary_index]);
      
      if (msg.is_insert == 0)
      {
        memcpy(&msg.summary, &summary, sizeof(SUMMARY));
        diff_len = encode_bitdiff((uint8_t *) trees[msg.tree_index].summary[msg.summary_index],
          (uint8_t *) &summary, sizeof(SUMMARY), msg.diff, sizeof(SUMMARY));
        if (diff_len >= 0)
        {
          msg.diff_len = diff_len;
          msg.is_diff = 1;
        } else
          msg.is_diff = 0;
      }

      memcpy(trees[msg.tree_index].summary[msg.summary_index], &summary, sizeof(SUMMARY));
      if (trees[msg.tree_index].hops > 0)
        send_treesummaryupdatemsg(&msg);
    } else
      DBGOUT("Summary has not changed");
  } else
    DBGOUT("Ignored, already received sequence number %d",
      peer_link[peer_index].tree.trees[msg.tree_index].update_seqnumber);
}

void init_mobility(ADDRESS *new_parents)
{
  TREESUMMARYUPDATE_MSG msg;
  int t, diff_len;

  msg.cmd = CMD_TREESUMMARYUPDATE;
  msg.source_node = TOS_NODE_ID;
  msg.summary_index = find_idsummary();

  msg.is_insert = 0;
  msg.seq_number = 1;
  summary_init(&msg.summary, summary_desc[msg.summary_index].type, summary_desc[msg.summary_index].type);  
  for (msg.tree_index = 0; msg.tree_index < TREE_COUNT; msg.tree_index++)
  {
    DBGOUT("Signing off from tree %d", msg.tree_index);
    diff_len = encode_bitdiff((uint8_t *) trees[msg.tree_index].summary[msg.summary_index],
      (uint8_t *) &msg.summary, sizeof(SUMMARY), msg.diff, sizeof(SUMMARY));
    if (diff_len >= 0)
    {
      msg.diff_len = diff_len;
      msg.is_diff = 1;
    } else
      msg.is_diff = 0;

    msg.new_parentnode = new_parents[msg.tree_index];
    send_treesummaryupdatemsg(&msg);
  }

  for (t = 0; t < TREE_COUNT; t++)
  {
    DBGOUT("Changing parent in %d from %d to %d", t, trees[t].parent, new_parents[t]);
    trees[t].parent = new_parents[t]; // need to update hops & deepest hop stats here
  }

  msg.is_diff = 0;
  msg.is_insert = 1;
  msg.seq_number = 2;
  msg.value = TOS_NODE_ID;
  for (msg.tree_index = 0; msg.tree_index < TREE_COUNT; msg.tree_index++)
  {
    DBGOUT("Joining tree %d", msg.tree_index);
    msg.new_parentnode = new_parents[msg.tree_index];
    send_treesummaryupdatemsg(&msg);
  }
}

void handle_treerootnotifymsg(ADDRESS source_id, uint8_t *p)
{                             // handler for tree root notification logical message
  TREEROOTNOTIFY_MSG msg;

  unmarshal_treerootnotifymsg(&msg, p);
  if (source_id != trees[tree_phase-1].parent)
  {
    DBGERR("Invalid parent: %d", source_id);
    return;
  }

  tree_phase = msg.tree;
  DBGOUT("Notify new root %d in tree %d received", msg.notify_id, msg.tree);  
  if (trees[tree_phase].hops != 0 && msg.notify_id == TOS_NODE_ID)
  {                                  // if root, then start tree construction for the new tree
    DBGOUT("Starting new tree construction");
    trees[tree_phase].hops = 0;
    tree_updated = 1;
    phase_start = timer_cycles;      // otherwise send to children, if summary is positive
  } else if (summary_test(trees[tree_phase-1].summary[ATTR_ID], msg.notify_id, 0))
    send_treerootnotifymsg(msg.notify_id);
}

uint8_t checkaccept_treerootnotifymsg(ADDRESS source_id, uint8_t *p)
{                        // confirmation handler, for tree root notify logical message
  TREEROOTNOTIFY_MSG msg;
                         // confirm only if we are supposed to start construction, or our summary is positive
  unmarshal_treerootnotifymsg(&msg, p);
  return (msg.notify_id == TOS_NODE_ID ||
    summary_test(trees[tree_phase-1].summary[ATTR_ID], msg.notify_id, 0));
}

void timer_treeconstruction()       // called from OS
{
  if (peer_discovery)
    if (timer_cycles < phase_start+DISCOVERY_EPOCHS || summarydesc_count == 0)  // do discovery if waiting for summaries
    {               // send one discovery message every 3 cycles, wait for previous to confirm
      if ((/*MIN_DISCOVERYMSG == 0 ||*/ timer_cycles % 3 == 0) && discoverymsg_sending == 0)
        send_discoverymsg(AM_BROADCAST_ADDR, MSG_NOCONFIRM, 1, 0);
    } else
      start_treeconstruct();
  else if (treemsg_sending == 0)
  {
    if (tree_updated && timer_cycles-treeupdate_time > 6)
    {
      if (timer_cycles % 3 == 0)       // if tree construction, send tree const. msg every 3 cycles, wait for previous to conf.
      {
        send_treeconstructmsg(tree_phase);
        tree_updated = 0;
      }
    } else if (trees[tree_phase].hops == 0 && timer_cycles >= phase_start+TREEEPOCH_COUNT &&
      trees[tree_phase].uncommitted_children == 0)      // when done with parent selection...
    {    
      set_globalstatus(GS_TREECOMMITWAIT);      
      tree_busy = 0;
      if (sensor_needsampling() == 0 && reset_requested != RESET_CODE && 
        (TOS_NODE_ID != BASESTATION_ADDR || uart_hassingletreemsgs() == 0))
        init_commit(); // wait for static attrs to sample, UART msgs with summary spec and attr updates to be sent before commit
    }         
  }  
}

